﻿using System;
using System.Collections.Generic;
using System.Collections;
using System.Linq;
using System.Text;
using System.IO;
using MotionPlanning.MotionPlanningComponents.MotionPlanningSystems.CollisionPrimitives;

namespace MotionPlanning.MotionPlanningComponents.MotionPlanningSystems
{
    public class System_SimplifiedPlanarPhysicalRobot : RoboticSystemComponent
    {
        protected List<Polygon_2D> Geometry_Environment;
        protected List<Polygon_2D> Geometry_Robot;

        public System_SimplifiedPlanarPhysicalRobot()
        {
        }

        public List<Polygon_2D> Get_Geometry_Environment()
        {
            return Geometry_Environment;
        }
        public List<Polygon_2D> Get_Geometry_Robot()
        {
            return Geometry_Robot;
        }

        virtual public bool CollisionFree(double[] q_test)
        {
            for (int i = 0; i < Geometry_Robot.Count; ++i)
            {
                for(int j=0;j<Geometry_Environment.Count;++j)
                {
                    bool collide = Geometry_Robot[i].IsCollidedWithIfAt(q_test, Geometry_Environment[j]);
                    if (collide == true)
                        return false;
                }
            }
            return true;
        }

        public override bool LoadingSystemParameters(string fileName)
        {
            base.LoadingSystemParameters(fileName);

            TextReader tr = new StreamReader(fileName);
            if (tr == null)
                throw new Exception("input file cannot be opened.");
            string rawFile = tr.ReadToEnd();

            //********************** read obstacles **********************
            string[] separators = new string[] { "<Geometry_Obstacles>", "</Geometry_Obstacles>" };
            string[] entries = rawFile.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            string roboticText = entries[1];

            string[] separatorsObject = new string[] {"<object>", "</object>" };
            entries = roboticText.Split(separatorsObject, StringSplitOptions.None);
            int n_objects = (entries.Length - 1) / 2;
            Geometry_Environment = new List<Polygon_2D>(n_objects);
            char[] trimCharactors = new char[] { '\r', '\t', '\n' };
            for (int i = 0; i < n_objects; ++i)
            {
                string cur_text = entries[1 + i * 2].Trim(trimCharactors);
                Geometry_Environment.Add(new Polygon_2D(cur_text));
            }
            //********************** read robot **********************
            separators = new string[] { "<Geometry_Robot>", "</Geometry_Robot>" };
            entries = rawFile.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            roboticText = entries[1];

            separatorsObject = new string[] { "<object>", "</object>" };
            entries = roboticText.Split(separatorsObject, StringSplitOptions.None);
            n_objects = (entries.Length - 1) / 2;

            Geometry_Robot = new List<Polygon_2D>(n_objects);
            trimCharactors = new char[] { '\r', '\t', '\n' };
            for (int i = 0; i < n_objects; ++i)
            {
                string cur_text = entries[1 + i * 2].Trim(trimCharactors);
                Geometry_Robot.Add(new Polygon_2D(cur_text));
            }
            //*********************************************************

            tr.Close();

            return true;
        }

        public void Transform(double[] src, double[] q, ref double[] dst)
        {
            double cosT = Math.Cos(q[2]);
            double sinT = Math.Sin(q[2]);
            dst[0] = q[0] + cosT * src[0] - sinT * src[1];
            dst[1] = q[1] + sinT * src[0] + cosT * src[1];
        }

        override public bool Propagate(RK4Helper pad, double[] q_start, double[] u, double duration, ref double[] q_end, ref double step_cost)
        {
            int n_step = (int)Math.Floor(duration / t_delta + 0.5);
            double[] q_in = new double[q_start.Length];
            MotionPlanningUtils.vector_set(q_start, ref q_in);
            MotionPlanningUtils.vector_set(q_start, ref q_end);
            step_cost = 0;
            for (int i = 0; i < n_step; ++i)
            {
                this.Integration(pad, q_in, u, t_delta, ref q_end);

                if (CollisionFree(q_end) == false)
                    return false;

                MotionPlanningUtils.vector_set(q_end, ref q_in);
                step_cost += t_delta;
            }
            return true;
        }

        override public bool Steering(double[] q_start, double[] q_end, ref double[] u, ref double duration, ref double edgeCost)
        {
            throw new NotImplementedException();
        }
    }
}
